#include "stdafx.h"
#include "ImageProcessing.h"

//#define VERBOSE


namespace OTPF {
	namespace Marker {

arma::colvec function (	arma::colvec		&x,		// Positionshypothese
								arma::colvec		&c,		// Kamerakonstante / Brennweite
								std::vector<arma::mat> &R,   // Rotationsmatrix
								std::vector<arma::colvec> &T // Translationsvektor Projektionszentrum
							   ) {
			
			#ifdef OLD
			std::vector<arma::colvec> dx (2);
			dx[0] = arma::colvec(2);
			dx[1] = arma::colvec(2);
			dx[0](0) = x(0) - T[0](0);
			dx[0](1) = x(1) - T[0](1);
			dx[0](2) = x(2) - T[0](2);
			dx[1](0) = x(0) - T[1](0);
			dx[1](1) = x(1) - T[1](1);
			dx[1](2) = x(2) - T[1](2);
			
			#ifdef VERBOSE
				dx[0].print("dx[0] = ");
				dx[1].print("dx[1] = ");
			#endif
			
			
			std::vector<double> N(2);
			for (int k = 0; k < 2; k++) {
				N[k] = R[k](0,2) * dx[k](0) + R[k](1,2) * dx[k](1) + R[k](2,2) * dx[k](2);
			}
			#ifdef VERBOSE
				mexPrintf("N[0] = %f\n", N[0]);
				mexPrintf("N[1] = %f\n", N[1]);
			#endif
			
			arma::colvec f = arma::zeros<arma::colvec>(4);
			f(0) = c(0) * ( R[0](0,0) * dx[0](0) + R[0](1,0) * dx[0](1) + R[0](2,0) * dx[0](2) ) / N[0];
			f(1) = c(0) * ( R[0](0,1) * dx[0](0) + R[0](1,1) * dx[0](1) + R[0](2,1) * dx[0](2) ) / N[0];
			f(2) = c(1) * ( R[1](0,0) * dx[1](0) + R[1](1,0) * dx[1](1) + R[1](2,0) * dx[1](2) ) / N[1];
			f(3) = c(1) * ( R[1](0,1) * dx[1](0) + R[1](1,1) * dx[1](1) + R[1](2,1) * dx[1](2) ) / N[1];
			#ifdef VERBOSE
				f.print("f = ");
			#endif
			
			return f;
			#else
			double dx1 = x(0) - T[0](0);
			double dy1 = x(1) - T[0](1);
			double dz1 = x(2) - T[0](2);
			double dx2 = x(0) - T[1](0);
			double dy2 = x(1) - T[1](1);
			double dz2 = x(2) - T[1](2);
			
			double r111 = R[0](0,0);
			double r121 = R[0](0,1);
			double r131 = R[0](0,2);
			double r211 = R[0](1,0);
			double r221 = R[0](1,1);
			double r231 = R[0](1,2); 
			double r311 = R[0](2,0); 
			double r321 = R[0](2,1);
			double r331 = R[0](2,2);
			
			double r112 = R[1](0,0);
			double r122 = R[1](0,1);
			double r132 = R[1](0,2);
			double r212 = R[1](1,0);
			double r222 = R[1](1,1);
			double r232 = R[1](1,2); 
			double r312 = R[1](2,0); 
			double r322 = R[1](2,1);
			double r332 = R[1](2,2);
			
			double kx1 = r111 * dx1 + r211 * dy1 + r311 * dz1;
			double kx2 = r112 * dx2 + r212 * dy2 + r312 * dz2;
			double ky1 = r121 * dx1 + r221 * dy1 + r321 * dz1;
			double ky2 = r122 * dx2 + r222 * dy2 + r322 * dz2;
		    
			double N1 = r131*(dx1)+r231*(dy1)+r331*(dz1);
			double N2 = r132*(dx2)+r232*(dy2)+r332*(dz2);
			
			arma::colvec f = arma::zeros<arma::colvec>(4);
		                
			f(0) = c(0) * kx1/N1;
			f(1) = c(0) * ky1/N1;
			f(2) = c(1) * kx2/N2;
			f(3) = c(1) * ky2/N2;
			
			return f;
			
			#endif
		}
	}
}